/**
 * Simple example intended to help users find the zero offset and natural direction of the sensor.
 *
 * These values can further be used to avoid motor and sensor alignment procedure.
 *
 * motor.initFOC(zero_offset, sensor_direction);
 *
 * This will only work for abosolute value sensors - magnetic sensors.
 * Bypassing the alignment procedure is not possible for the encoders and for the current implementation of the Hall sensors.
 * library version 1.4.2.
 *
 */
#include <SimpleFOC.h>

// magnetic sensor instance - SPI
// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
// magnetic sensor instance - I2C
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);
TwoWire I2Ctwo = TwoWire(1);
// magnetic sensor instance - analog output
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);

// BLDC motor instance
//电机参数
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(32, 14, 25, 12); // M1电机

BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(27, 26, 33, 13); // M2电机
// Stepper motor instance
// StepperMotor motor = StepperMotor(50);
// StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);

void setup()
{

	// power supply voltage
	driver.voltage_power_supply = 24;
	driver1.voltage_power_supply = 24;
	driver.init();
	driver1.init();
	motor.linkDriver(&driver);
	motor1.linkDriver(&driver1);

	// initialise magnetic sensor hardware
	sensor.init(&I2Cone);
	sensor1.init(&I2Ctwo);
	// link the motor to the sensor
	motor.linkSensor(&sensor);
	motor1.linkSensor(&sensor);

	// aligning voltage
	motor.voltage_sensor_align = 7;
	motor1.voltage_sensor_align = 7;

	// set motion control loop to be used
	motor.controller = MotionControlType::torque;
	motor1.controller = MotionControlType::torque;

	// initialize motor
	motor.init();
	motor1.init();
	// align sensor and start FOC
	motor.initFOC();
	motor1.initFOC();

	Serial.begin(115200);
	Serial.println("Sensor zero offset is:");
	Serial.println(motor.zero_electric_angle, 4);
	Serial.println("Sensor natural direction is: ");
	Serial.println(motor.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW");

	Serial.println("Sensor1 zero offset is:");
	Serial.println(motor1.zero_electric_angle, 4);
	Serial.println("Sensor1 natural direction is: ");
	Serial.println(motor1.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW");

	Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)");
	_delay(1000);
	Serial.println("If motor is not moving the alignment procedure was not successfull!!");
}

void loop()
{

	// main FOC algorithm function
	motor.loopFOC();
	motor1.loopFOC();

	// Motion control function
	motor.move(2);
	motor1.move(2);
}
